#include <windows.h>
#include <tchar.h>
#include "VisualizeSurfaceMesh.h"
#include "PlaneFitting_FVI.h"
#include "Kinect.h"
#include "Visualizer.h"


float3 VisualizeSurfaceMesh::ViewPoint = {0.0, 0.0, 0.0};
float3 VisualizeSurfaceMesh::ViewAngle = {0.0, 0.0, 0.0};
PlaneFitting_FVI* VisualizeSurfaceMesh::FVI = 0;
GLuint VisualizeSurfaceMesh::_textureId = 0;
const float VisualizeSurfaceMesh::MaxMeshSize = 150.0;
const int VisualizeSurfaceMesh::WindowNum = 3;
int* VisualizeSurfaceMesh::WindowID = 0;
GLCapture VisualizeSurfaceMesh::Capture1;
GLCapture VisualizeSurfaceMesh::Capture2;
GLCapture VisualizeSurfaceMesh::Capture3;
//cv::VideoWriter VisualizeSurfaceMesh::Writer("superpixel.avi", CV_FOURCC('X','V','I','D'), 5.0, cv::Size(Kinect::Width, Kinect::Height));
cv::Mat_<cv::Vec3b> VisualizeSurfaceMesh::SegmentedImage(Kinect::Width, Kinect::Height);
Visualizer* VisualizeSurfaceMesh::Visual = 0;

VisualizeSurfaceMesh::VisualizeSurfaceMesh(){
	FVI = new PlaneFitting_FVI();
	WindowID = new int[WindowNum];
	Visual = new Visualizer(Kinect::Width, Kinect::Height);
}
VisualizeSurfaceMesh::~VisualizeSurfaceMesh(){
	delete FVI;
	FVI = 0;
	delete WindowID;
	WindowID = 0;
	delete Visual;
	Visual = 0;
}
void VisualizeSurfaceMesh::Process(int argc, char** argv){
	glutInit(&argc, argv);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(Kinect::Width, Kinect::Height);
	//Window0
	WindowID[0] = glutCreateWindow("PlaneFitted");
	Capture1.setWriteFile("PlaneFitted");
	initRendering();

	glutDisplayFunc(renderOutputTexture);
	glutKeyboardFunc(handleKeypress);
	glutReshapeFunc(handleResize);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(Kinect::Width, Kinect::Height);
	//Window1
	WindowID[1] = glutCreateWindow("Filtered");
	Capture2.setWriteFile("Filtered");
	initRendering();

	glutDisplayFunc(renderInputTexture);
	glutKeyboardFunc(handleKeypress);
	glutReshapeFunc(handleResize);

	//Window2
	WindowID[2] = glutCreateWindow("Segmented");
	Capture3.setWriteFile("Segmented");
	initRendering();

	glutDisplayFunc(renderSegmentedTexture);
	glutKeyboardFunc(handleKeypress);
	glutReshapeFunc(handleResize);

	glutIdleFunc(idle);
	glutMainLoop();
}

void VisualizeSurfaceMesh::handleKeypress(unsigned char key, int x, int y) {

	switch (key) {
		//right
	case 'r':
		ViewPoint.x -= 20.0;
		break;		   
		//left			   
	case 'l':		   
		ViewPoint.x += 20.0;
		break;		   
		//up			   
	case 'u':		   
		ViewPoint.y -= 20.0;
		break;		   
		//down			   
	case 'd':		   
		ViewPoint.y += 20.0;
		break;		   
		//zoom in +		   
	case '+':		   
		ViewPoint.z += 20.0;
		break;		   
		//zoom out -	   
	case '-':		   
		ViewPoint.z -= 20.0;
		break;
		//rotate left
	case 'L':
		ViewAngle.y -= 1.0;
		break;
		//rotate right
	case 'R':
		ViewAngle.y += 1.0;
		break;
		//rotate up
	case 'U':
		ViewAngle.x -= 1.0;
		break;
		//rotate down
	case 'D':
		ViewAngle.x += 1.0;
		break;
	case 'q':
		ViewPoint.x = 0.0;
		ViewPoint.y = 0.0;
		ViewPoint.z = 0.0;
		ViewAngle.x = 0.0;
		ViewAngle.y = 0.0;
		ViewAngle.z = 0.0;
		break;

	case 27: //Escape key
		FVI->releaseVideo();
		exit(0);
	}
}

//Makes the image into a texture, and returns the id of the texture
GLuint VisualizeSurfaceMesh::loadTexture(const cv::Mat_<cv::Vec3b> image){
	GLuint textureId;
	glGenTextures(1, &textureId); //Make room for our texture
	glBindTexture(GL_TEXTURE_2D, textureId); //Tell OpenGL which texture to edit
	//Map the image to the texture
	glTexImage2D(GL_TEXTURE_2D,                //Always GL_TEXTURE_2D
		0,                            //0 for now
		GL_RGB,                       //Format OpenGL uses for image
		image.cols, image.rows,  //Width and height
		0,                            //The border of the image
		GL_BGR_EXT, //GL_RGB, because pixels are stored in RGB format
		GL_UNSIGNED_BYTE, //GL_UNSIGNED_BYTE, because pixels are stored
		//as unsigned numbers
		image.data);               //The actual pixel data
	return textureId; //Returns the id of the texture
}



void VisualizeSurfaceMesh::initRendering() {
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_NORMALIZE);
	glEnable(GL_COLOR_MATERIAL);
}

void VisualizeSurfaceMesh::handleResize(int w, int h) {
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45.0, (float)w / (float)h, 1.0, 10000.0);
}
void VisualizeSurfaceMesh::idle(){
	for(int i=0; i<WindowNum; i++){
		glutSetWindow(WindowID[i]);
		glutPostRedisplay();
	}
}
void VisualizeSurfaceMesh::renderOutputTexture() {
	//Process
	FVI->PlaneFitting();
	//GetImage
	_textureId = loadTexture(FVI->GetColorImage());

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	//start += 10.0;
	//angle -= 0.1;
	glTranslatef(ViewPoint.x, ViewPoint.y, ViewPoint.z);
	glRotatef(ViewAngle.y, 0.0, 1.0, 0.0);
	glRotatef(ViewAngle.x, 1.0, 0.0, 0.0);
	//glTranslatef(0.0f, 1.0f, 10.0f+start);
	//glRotatef(angle, 0.0, 1.0, 0.0);
	//glRotatef(angle, 1.0, 0.0, 0.0);
	GLfloat ambientLight[] = {0.2f, 0.2f, 0.2f, 1.0f};
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientLight);

	GLfloat directedLight[] = {5.0f, 5.0f, 5.0f, 1.0f};
	GLfloat directedLightPos[] = {-10.0f, 15.0f, 20.0f, 0.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, directedLight);
	glLightfv(GL_LIGHT0, GL_POSITION, directedLightPos);

	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, _textureId);

	//Bottom
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glColor3f(1.0f, 0.2f, 0.2f);
	for(int y=0; y<(Kinect::Height-1); y++){
		for(int x=0; x<(Kinect::Width-1); x++){
			//std::cout <<"x: "<<FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].x << 
			//", y: "<< FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].y <<
			//", z: "<< FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z <<std::endl;
			//triangle mesh 1
			if(FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z > 50.0){
					if(fabs(FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z - FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z - FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z - FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z) < MaxMeshSize ){
							//if(FVI->GetClusterLabel()->at<int>(y, x) == FVI->GetClusterLabel()->at<int>(y, x+1) &&
							//	FVI->GetClusterLabel()->at<int>(y, x+1) == FVI->GetClusterLabel()->at<int>(y+1, x)){ 
							glBegin(GL_TRIANGLES);

							glTexCoord2f((float)x/(float)Kinect::Width, (float)y/(float)Kinect::Height);
							glVertex3f(FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].x, FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].y, -FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z);

							glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)y/(float)Kinect::Height);
							glVertex3f(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].x, FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].y, -FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z);

							glTexCoord2f((float)x/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
							glVertex3f(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].x, FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].y, -FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z);

							glEnd();
							//	}
					}
			}
			//triangle mesh 2
			if(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z > 50.0){
					if(fabs(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z - FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z - FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z - FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z) < MaxMeshSize ){
							//if(FVI->GetClusterLabel()->at<int>(y, x+1) == FVI->GetClusterLabel()->at<int>(y+1, x) &&
							//	FVI->GetClusterLabel()->at<int>(y+1, x) == FVI->GetClusterLabel()->at<int>(y+1, x+1)){
							glBegin(GL_TRIANGLES);
							glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)y/(float)Kinect::Height);
							glVertex3f(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].x, FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].y, -FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z);

							glTexCoord2f((float)x/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
							glVertex3f(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].x, FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].y, -FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z);

							glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
							glVertex3f(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].x, FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].y, -FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z);

							glEnd();
							//}
					}
			}
		}
	}
	glDisable(GL_TEXTURE_2D);

	////Back
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	//glColor3f(1.0f, 1.0f, 1.0f);
	//glBegin(GL_TRIANGLES);
	//
	//glNormal3f(0.0f, 0.0f, 1.0f);
	//glTexCoord2f(0.0f, 0.0f);
	//glVertex3f(-2.5f, -2.5f, -2.5f);
	//glTexCoord2f(5.0f, 5.0f);
	//glVertex3f(0.0f, 2.5f, -2.5f);
	//glTexCoord2f(10.0f, 0.0f);
	//glVertex3f(2.5f, -2.5f, -2.5f);
	//
	//glEnd();
	//
	////Left
	//glDisable(GL_TEXTURE_2D);
	//glColor3f(1.0f, 0.7f, 0.3f);
	//glBegin(GL_QUADS);
	//
	//glNormal3f(1.0f, 0.0f, 0.0f);
	//glVertex3f(-2.5f, -2.5f, 2.5f);
	//glVertex3f(-2.5f, -2.5f, -2.5f);
	//glVertex3f(-2.5f, 2.5f, -2.5f);
	//glVertex3f(-2.5f, 2.5f, 2.5f);
	//
	//glEnd();

	glutSwapBuffers();

	Capture1.write();
}

void VisualizeSurfaceMesh::renderInputTexture() {
	//Process
	//FVI->PlaneFitting();
	//GetImage
	_textureId = loadTexture(FVI->GetColorImage());

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	//start += 10.0;
	//angle -= 0.1;
	//glTranslatef(0.0f, 1.0f, 10.0f+start);
	//glRotatef(angle, 0.0, 1.0, 0.0);
	//glRotatef(angle, 1.0, 0.0, 0.0);
	glTranslatef(ViewPoint.x, ViewPoint.y, ViewPoint.z);
	glRotatef(ViewAngle.y, 0.0, 1.0, 0.0);
	glRotatef(ViewAngle.x, 1.0, 0.0, 0.0);
	GLfloat ambientLight[] = {0.2f, 0.2f, 0.2f, 1.0f};
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientLight);

	GLfloat directedLight[] = {5.0f, 5.0f, 5.0f, 1.0f};
	GLfloat directedLightPos[] = {-10.0f, 15.0f, 20.0f, 0.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, directedLight);
	glLightfv(GL_LIGHT0, GL_POSITION, directedLightPos);

	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, _textureId);

	//Bottom
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glColor3f(1.0f, 0.2f, 0.2f);
	for(int y=0; y<(Kinect::Height-1); y++){
		for(int x=0; x<(Kinect::Width-1); x++){
			/*std::cout <<"x: "<<FVI->GetFiltered_Host()[y*Kinect::Width+x].x << 
			", y: "<< FVI->GetFiltered_Host()[y*Kinect::Width+x].y <<
			", z: "<< FVI->GetFiltered_Host()[y*Kinect::Width+x].z <<std::endl;*/
			//triangle mesh 1
			if(FVI->GetFiltered_Host()[y*Kinect::Width+x].z > 50.0 &&
				FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z > 50.0 &&
				FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z > 50.0){
					if(fabs(FVI->GetFiltered_Host()[y*Kinect::Width+x].z - FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z) < MaxMeshSize &&
						fabs(FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z - FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z) < MaxMeshSize &&
						fabs(FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z - FVI->GetFiltered_Host()[y*Kinect::Width+x].z) < MaxMeshSize ){
							glBegin(GL_TRIANGLES);

							glTexCoord2f((float)x/(float)Kinect::Width, (float)y/(float)Kinect::Height);
							glVertex3f(FVI->GetFiltered_Host()[y*Kinect::Width+x].x, FVI->GetFiltered_Host()[y*Kinect::Width+x].y, -FVI->GetFiltered_Host()[y*Kinect::Width+x].z);

							glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)y/(float)Kinect::Height);
							glVertex3f(FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].x, FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].y, -FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z);

							glTexCoord2f((float)x/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
							glVertex3f(FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].x, FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].y, -FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z);

							glEnd();
					}
			}
			//triangle mesh 2
			if(FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z > 50.0 &&
				FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z > 50.0 &&
				FVI->GetFiltered_Host()[(y+1)*Kinect::Width+(x+1)].z > 50.0){
					if(fabs(FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z - FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z) < MaxMeshSize &&
						fabs(FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z - FVI->GetFiltered_Host()[(y+1)*Kinect::Width+(x+1)].z) < MaxMeshSize &&
						fabs(FVI->GetFiltered_Host()[(y+1)*Kinect::Width+(x+1)].z - FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z) < MaxMeshSize ){

							glBegin(GL_TRIANGLES);
							glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)y/(float)Kinect::Height);
							glVertex3f(FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].x, FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].y, -FVI->GetFiltered_Host()[y*Kinect::Width+(x+1)].z);

							glTexCoord2f((float)x/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
							glVertex3f(FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].x, FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].y, -FVI->GetFiltered_Host()[(y+1)*Kinect::Width+x].z);

							glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
							glVertex3f(FVI->GetFiltered_Host()[(y+1)*Kinect::Width+(x+1)].x, FVI->GetFiltered_Host()[(y+1)*Kinect::Width+(x+1)].y, -FVI->GetFiltered_Host()[(y+1)*Kinect::Width+(x+1)].z);

							glEnd();
					}
			}
		}
	}
	glDisable(GL_TEXTURE_2D);

	////Back
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	//glColor3f(1.0f, 1.0f, 1.0f);
	//glBegin(GL_TRIANGLES);
	//
	//glNormal3f(0.0f, 0.0f, 1.0f);
	//glTexCoord2f(0.0f, 0.0f);
	//glVertex3f(-2.5f, -2.5f, -2.5f);
	//glTexCoord2f(5.0f, 5.0f);
	//glVertex3f(0.0f, 2.5f, -2.5f);
	//glTexCoord2f(10.0f, 0.0f);
	//glVertex3f(2.5f, -2.5f, -2.5f);
	//
	//glEnd();
	//
	////Left
	//glDisable(GL_TEXTURE_2D);
	//glColor3f(1.0f, 0.7f, 0.3f);
	//glBegin(GL_QUADS);
	//
	//glNormal3f(1.0f, 0.0f, 0.0f);
	//glVertex3f(-2.5f, -2.5f, 2.5f);
	//glVertex3f(-2.5f, -2.5f, -2.5f);
	//glVertex3f(-2.5f, 2.5f, -2.5f);
	//glVertex3f(-2.5f, 2.5f, 2.5f);
	//
	//glEnd();

	glutSwapBuffers();

	Capture2.write();
}

void VisualizeSurfaceMesh::renderSegmentedTexture(){
	//copy image
	SegmentedImage = FVI->GetColorImage();
	//for(int y=1; y<(Kinect::Height-1); y++){
	//	for(int x=1; x<(Kinect::Width-1); x++){
	//		//segmeted 2D image
	//		for(int i = y - 1; i <= y + 1; i++){
	//			for(int j = x - 1; j <= x + 1; j++){
	//				std::cout << "x: "<<x <<", y: "<<y<< " label: "<<FVI->GetClusterLabel()->at<int>(y, x)<<std::endl;
	//				if(FVI->GetClusterLabel()->at<int>(y, x) != FVI->GetClusterLabel()->at<int>(i, j)){
	//					SegmentedImage.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);
	//				}
	//			}
	//		}
	//	}
	//}
	//Writer << SegmentedImage;
	//cv::imshow("segmented image", SegmentedImage);
	//cv::waitKey(1);
	//Visual->display(*FVI->GetColorImage(), FVI->GetClusterLabel(), "segmented_image");
	//GetImage
	_textureId = loadTexture(FVI->GetColorImage());

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	//start += 10.0;
	//angle -= 0.1;
	glTranslatef(ViewPoint.x, ViewPoint.y, ViewPoint.z);
	glRotatef(ViewAngle.y, 0.0, 1.0, 0.0);
	glRotatef(ViewAngle.x, 1.0, 0.0, 0.0);
	//glTranslatef(0.0f, 1.0f, 10.0f+start);
	//glRotatef(angle, 0.0, 1.0, 0.0);
	//glRotatef(angle, 1.0, 0.0, 0.0);
	GLfloat ambientLight[] = {0.2f, 0.2f, 0.2f, 1.0f};
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientLight);

	GLfloat directedLight[] = {5.0f, 5.0f, 5.0f, 1.0f};
	GLfloat directedLightPos[] = {-10.0f, 15.0f, 20.0f, 0.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, directedLight);
	glLightfv(GL_LIGHT0, GL_POSITION, directedLightPos);

	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, _textureId);

	//Bottom
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glColor3f(1.0f, 0.2f, 0.2f);
	for(int y=0; y<(Kinect::Height-1); y++){
		for(int x=0; x<(Kinect::Width-1); x++){
			//triangle mesh 1
			if(FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z > 50.0){
					if(fabs(FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z - FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z - FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z - FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z) < MaxMeshSize ){
							if(FVI->GetClusterLabel().at<int>(y, x) == FVI->GetClusterLabel().at<int>(y, x+1) &&
								FVI->GetClusterLabel().at<int>(y, x+1) == FVI->GetClusterLabel().at<int>(y+1, x)){ 
									glBegin(GL_TRIANGLES);

									glTexCoord2f((float)x/(float)Kinect::Width, (float)y/(float)Kinect::Height);
									glVertex3f(FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].x, FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].y, -FVI->GetPlaneFitted_Host()[y*Kinect::Width+x].z);

									glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)y/(float)Kinect::Height);
									glVertex3f(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].x, FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].y, -FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z);

									glTexCoord2f((float)x/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
									glVertex3f(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].x, FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].y, -FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z);

									glEnd();
							}
					}
			}
			//triangle mesh 2
			if(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z > 50.0 &&
				FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z > 50.0){
					if(fabs(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z - FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z - FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z) < MaxMeshSize &&
						fabs(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z - FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z) < MaxMeshSize ){
							if(FVI->GetClusterLabel().at<int>(y, x+1) == FVI->GetClusterLabel().at<int>(y+1, x) &&
								FVI->GetClusterLabel().at<int>(y+1, x) == FVI->GetClusterLabel().at<int>(y+1, x+1)){
									glBegin(GL_TRIANGLES);
									glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)y/(float)Kinect::Height);
									glVertex3f(FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].x, FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].y, -FVI->GetPlaneFitted_Host()[y*Kinect::Width+(x+1)].z);

									glTexCoord2f((float)x/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
									glVertex3f(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].x, FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].y, -FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+x].z);

									glTexCoord2f((float)(x+1)/(float)Kinect::Width, (float)(y+1)/(float)Kinect::Height);
									glVertex3f(FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].x, FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].y, -FVI->GetPlaneFitted_Host()[(y+1)*Kinect::Width+(x+1)].z);

									glEnd();
							}
					}
			}
			//if(x > 0 && y > 0){
			////segmented 2D image
			//for(int i = y - 1; i <= y + 1; i++){
			//	for(int j = x - 1; j <= x + 1; j++){
			//		//std::cout << "x: "<<x <<", y: "<<y<< " label: "<<FVI->GetClusterLabel()->at<int>(y, x)<<std::endl;
			//		if(FVI->GetClusterLabel()->at<int>(y, x) != FVI->GetClusterLabel()->at<int>(i, j)){
			//			SegmentedImage.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);
			//		}
			//	}
			//}
			//}
		}
	}
	//Writer << SegmentedImage;
	//cv::imshow("segmented image", SegmentedImage);
	//cv::waitKey(1);

	glDisable(GL_TEXTURE_2D);
	
	////Back
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	//glColor3f(1.0f, 1.0f, 1.0f);
	//glBegin(GL_TRIANGLES);
	//
	//glNormal3f(0.0f, 0.0f, 1.0f);
	//glTexCoord2f(0.0f, 0.0f);
	//glVertex3f(-2.5f, -2.5f, -2.5f);
	//glTexCoord2f(5.0f, 5.0f);
	//glVertex3f(0.0f, 2.5f, -2.5f);
	//glTexCoord2f(10.0f, 0.0f);
	//glVertex3f(2.5f, -2.5f, -2.5f);
	//
	//glEnd();
	//
	////Left
	//glDisable(GL_TEXTURE_2D);
	//glColor3f(1.0f, 0.7f, 0.3f);
	//glBegin(GL_QUADS);
	//
	//glNormal3f(1.0f, 0.0f, 0.0f);
	//glVertex3f(-2.5f, -2.5f, 2.5f);
	//glVertex3f(-2.5f, -2.5f, -2.5f);
	//glVertex3f(-2.5f, 2.5f, -2.5f);
	//glVertex3f(-2.5f, 2.5f, 2.5f);
	//
	//glEnd();

	glutSwapBuffers();

	Capture3.write();


}